//！ 三维空间算法
const lbmath = @import("./matrix/math/lbmath.zig");
// const zlbmath = @import("zlbmath_lib");
// const lbmath = zlbmath.lbmath;
const std = @import("std");
const Math_Compute_Abandon = lbmath.Math_Compute_Abandon;
const dprint = std.debug.print;
const testing = std.testing;
const math = std.math;
const lbmatrix = @import("./matrix/lbmatrix.zig");
const threed_geometry_tool = @import("./threed_geometry_tool.zig");
/// 处理不等式约束 Fx <= f
/// 如果 x 的下一步更新 tx 违反了约束，则将 tx 投影回可行域
fn handleInequalityConstraint(
    comptime T: type,
    x: []const T, // 当前解
    tx: []T, // 当前的更新步长 (可变)
    F: []const []const T, // 约束矩阵
    f: []const T, // 约束向量
) void {
    const col = x.len;
    for (F, f) |F_row, f_val| {
        // 计算 temp_d = F_row * (x + tx) - f_val
        var temp_d: T = -f_val;
        for (0..col) |k| {
            temp_d += F_row[k] * (x[k] + tx[k]);
        }

        // 如果 temp_d > 0，说明违反了约束 F_row * x <= f_val
        if (temp_d > 0) {
            const inn = lbmath.inner_product(F_row, F_row); // ||F_row||^2
            const scale = temp_d / inn;
            for (0..col) |k| {
                tx[k] -= scale * F_row[k];
            }
        }
    }
}

// 计算当前误差 ||Ax - b||^2
// 对应 C 代码中的 compute_error_gdse
fn computeError(
    comptime T: type,
    A: []const []const T,
    x: []const T,
    b: []const T,
) T {
    var total_error: T = 0;
    for (A, b) |A_row, b_val| {
        // 计算 (A_row * x) - b_val
        const temp = lbmath.inner_product(A_row, x) - b_val;
        total_error += (temp * temp);
    }
    return total_error;
}

/// 计算 A^T * A
/// 对应 C 代码中的 compute_ATA
/// 由于 A^T * A 是对称矩阵，这里做了优化，只计算上三角部分
fn computeATA(
    comptime T: type,
    A: []const []const T,
    re: []const []T, // 输出矩阵 A^T * A
) void {
    const row = A.len;
    if (row == 0) return;
    const col = A[0].len;

    for (0..col) |i| {
        for (i..col) |j| {
            var temp: T = 0;
            for (0..row) |k| {
                temp += A[k][i] * A[k][j];
            }
            re[i][j] = temp;
            re[j][i] = temp; // 对称赋值
        }
    }
}

/// 使用带动量的梯度下降法求解线性方程组 Ax=b，并满足不等式约束 Fx<=f。
/// 目标是最小化 ||Ax - b||^2。
///
/// @param T         浮点类型，如 f32 或 f64。
/// @param allocator 用于分配临时内存的分配器。
/// @param A         输入矩阵，类型为 [][]T。
/// @param b         输入向量，类型为 []T。
/// @param x         解向量 (in-out)，提供初始猜测，函数会就地修改它。
/// @param F         可选的不等式约束矩阵 F。
/// @param f         可选的不等式约束向量 f。
/// @param delta     梯度下降的学习率。
/// @param num       最大迭代次数。
/// @return          返回可能的内存分配错误，或者计算完成后的最终误差。
pub fn gradientDescentForSolvingSystemsOfEquations(
    comptime T: type,
    allocator: std.mem.Allocator,
    A: []const []const T,
    b: []const T,
    x: []T, // in-out 参数
    F: ?[]const []const T,
    f: ?[]const T,
    delta: T,
    num: usize,
) !T {
    const row = A.len;
    if (row == 0) return 0.0;
    const col = A[0].len;

    // 类型和维度断言，确保输入是合法的
    std.debug.assert(b.len == row);
    std.debug.assert(x.len == col);

    // --- 内存分配 ---
    // 为 A^T*A 分配内存 (col x col)
    var ATA_buffer = try allocator.alloc(T, col * col);
    defer allocator.free(ATA_buffer);
    // 创建一个指向该缓冲区的二维切片视图
    var ATA = try allocator.alloc([]T, col);
    defer allocator.free(ATA);
    for (0..col) |i| {
        ATA[i] = ATA_buffer[i * col .. (i + 1) * col];
    }

    // 为 A^T*b 分配内存 (col)
    const ATb = try allocator.alloc(T, col);
    defer allocator.free(ATb);

    // 为动量/更新向量 tx 分配内存 (col)
    const tx = try allocator.alloc(T, col);
    defer allocator.free(tx);
    // std.mem.zeroes(tx); // **重要**: 初始化动量为0，修复了C代码中的bug
    @memset(tx, 0);
    // --- 预计算 ---
    // 梯度公式为 2 * (A^T*A*x - A^T*b)，我们预计算不变的部分
    // 计算 A^T*b
    // std.mem.zeroes(ATb);
    @memset(ATb, 0);
    for (0..col) |i| {
        for (0..row) |j| {
            ATb[i] += A[j][i] * b[j];
        }
    }
    // 计算 A^T*A
    computeATA(T, A, ATA);

    // --- 迭代求解 ---
    const momentum_coe: T = 0.6;

    for (0..num) |_| {
        const erro = computeError(T, A, x, b);
        // 使用 std.debug.print 在调试时打印信息
        // std.debug.print("Iteration {d}: error = {d:.10}\n", .{ i, erro });
        // if (std.math.fabs(erro) < 1e-11) {
        if (lbmath.approxEqAbs(T, erro, 0, null)) {
            break;
        }

        // 梯度计算与动量更新
        // C代码: tx[j] = tx[j]*momentum_coe - delta*(tempd - ATb[j]);
        // 其中 tempd = (A^T*A*x)[j]
        for (0..col) |j| {
            const tempd = lbmath.inner_product(ATA[j], x);
            tx[j] = tx[j] * momentum_coe - delta * (tempd - ATb[j]);
        }

        // 如果存在不等式约束，则处理它们
        if (F) |F_matrix| {
            if (f) |f_vector| {
                // 维度断言
                std.debug.assert(F_matrix.len == f_vector.len);
                if (F_matrix.len > 0) {
                    std.debug.assert(F_matrix[0].len == col);
                }
                handleInequalityConstraint(T, x, tx, F_matrix, f_vector);
            }
        }

        // 更新解向量 x
        for (0..col) |j| {
            x[j] += tx[j];
        }
    }

    return computeError(T, A, x, b);
}

/// 求解带约束的二次规划问题 (Quadratic Programming Problem)
///
/// 最小化: 0.5 * x^T * H * x + C^T * x
/// subject to:
///   A * x <= b  (不等式约束)
///   E * x =  d  (等式约束)
///
/// @param T         浮点类型，如 f32 或 f64。
/// @param allocator 用于分配临时内存的分配器。
/// @param H         二次项的对称矩阵 (n x n)。
/// @param C         线性项的向量 (n)。
/// @param A         不等式约束矩阵 (m x n)。
/// @param b         不等式约束向量 (m)。
/// @param E         等式约束矩阵 (p x n)。
/// @param d         等式约束向量 (p)。
/// @param x         解向量 (out)，函数会将结果写入此切片。
/// @return          返回可能的内存分配错误，或者计算完成后的最终误差。
pub fn optQuadraticProblemWithConstraint(
    comptime T: type,
    allocator: std.mem.Allocator,
    H: []const []const T,
    C: []const T,
    A: ?[]const []const T, // 不等式: Ax <= b
    b: ?[]const T,
    E: ?[]const []const T, // 等式: Ex = d
    d: ?[]const T,
    x: []T, // 输出
    num: usize,
    delta: T,
) !T {
    const len = H.len;
    std.debug.assert(len > 0 and H[0].len == len);
    std.debug.assert(C.len == len);
    std.debug.assert(x.len == len);

    // const len_a = if (A) |a_mat| a_mat.len else 0;
    if (E) |e_mat| {
        const len_e = e_mat.len;
        const dia_x: []bool = try allocator.alloc(bool, len);
        defer allocator.free(dia_x);
        const ea = try allocator.alloc([]T, len_e);
        defer allocator.free(ea);
        for (0..len_e) |i| {
            ea[i] = try allocator.alloc(T, len);
        }
        defer {
            for (0..len_e) |i| {
                allocator.free(ea[i]);
            }
        }
        const db: ?[]T = blk: {
            if (d) |d_vec| {
                try allocator.alloc(T, d_vec.len);
            } else {
                break :blk null;
            }
        };
        defer {
            if (db) |db_vec| allocator.free(db_vec);
        }
        if (!lbmatrix.diagonalizable_matrix(
            T,
            ea,
            db,
            null,
            dia_x,
        )) {
            return lbmath.Math_Compute_Abandon.Math_Compute_Abandon;
        }
        const rex = try allocator.alloc([]T, len);
        defer allocator.free(rex);
        var new_len: usize = 0;
        for (dia_x) |dia_xv| {
            if (dia_xv) {
                new_len += 1;
            }
        }
        for (0..len) |i| {
            rex[i] = try allocator.alloc(T, new_len);
        }
        defer {
            for (0..len) |i| {
                allocator.free(rex[i]);
            }
        }
        const rex_b = try allocator.alloc(T, len);
        defer allocator.free(rex_b);

        lbmatrix.solve_Ax_b(
            T,
            ea,
            db,
            dia_x,
            rex,
            rex_b,
        );
        const nx = try allocator.alloc(T, new_len);
        defer allocator.free(nx);
        const NH = try allocator.alloc([]T, new_len);
        defer allocator.free(NH);
        for (0..new_len) |i| {
            NH[i] = try allocator.alloc(T, new_len);
        }
        defer {
            for (0..new_len) |i| {
                allocator.free(NH[i]);
            }
        }
        const NC = try allocator.alloc(T, new_len);
        defer allocator.free(NC);

        const temp_vec = try allocator.alloc(T, len);
        defer allocator.free(temp_vec);
        for (0..new_len) |i| {
            for (0..len) |k| {
                temp_vec[k] = 0;
                for (0..len) |l| {
                    temp_vec[k] += A[l][k] * rex[l][i];
                }
            }
            for (0..new_len) |j| {
                NH[i][j] = 0;
                for (0..len) |k| {
                    NH[i][j] += rex[k][j] * temp_vec[k]; //rex[l][i] * A[l][k];
                }
            }
        }
        for (0..new_len) |i| {
            for (0..len) |j| {
                temp_vec[j] = 0;
                for (0..len) |k| {
                    temp_vec[j] += H[j][k] * rex[k][i];
                }
            }
            NC[i] = 0;
            for (0..len) |j| {
                NC[i] += temp_vec[j] * rex_b[j];
                NC[i] += C[j] * rex[j][i];
            }
        }
        const NA = try allocator.alloc([]T, A.len);
        defer allocator.free(NA);
        for (0..NA.len) |i| {
            NA[i] = try allocator.alloc(T, new_len);
        }
        defer {
            for (0..NA.len) |i| {
                allocator.free(NA[i]);
            }
        }
        try lbmatrix.mult_matrix(A, rex, NA);
        const erro = try optQuadraticProblemWithConstraint(
            T,
            allocator,
            NH,
            NC,
            NA, // 不等式: Ax <= b
            b,
            null, // 等式: Ex = d
            null,
            nx, // 输出
            30,
            delta,
        );
        try lbmatrix.matrix_mult_array(rex, nx, x);

        return erro;
    } else {
        // 为动量/更新向量 tx 分配内存 (col)
        const tx = try allocator.alloc(T, len);
        defer allocator.free(tx);
        // std.mem.zeroes(tx); // **重要**: 初始化动量为0，修复了C代码中的bug
        @memset(tx, 0);

        // --- 迭代求解 ---
        const momentum_coe: T = 0.6;

        for (0..num) |_| {

            // 梯度计算与动量更新
            // C代码: tx[j] = tx[j]*momentum_coe - delta*(tempd - ATb[j]);
            // 其中 tempd = (A^T*A*x)[j]
            for (0..len) |j| {
                const tempd = lbmath.inner_product(H[j], x);
                tx[j] = tx[j] * momentum_coe - delta * (tempd + C[j]);
            }

            // 如果存在不等式约束，则处理它们
            if (A) |A_matrix| {
                if (b) |b_vector| {
                    // 维度断言
                    std.debug.assert(A_matrix.len == b_vector.len);
                    if (A_matrix.len > 0) {
                        std.debug.assert(A_matrix[0].len == len);
                    }
                    handleInequalityConstraint(T, x, tx, A_matrix, b_vector);
                }
            }

            // 更新解向量 x
            for (0..len) |j| {
                x[j] += tx[j];
            }
            const erro = lbmath.inner_product(tx, tx);
            if (lbmath.approxEqAbs(T, erro, 0, null)) {
                return erro;
            }
        }
    }
    // const len_e = if (E) |e_mat| e_mat.len else 0;

    return 0;
}

// test optQuadraticProblemWithConstraint {

//     // 1. 定义与 C 代码等效的输入数据
//     // C 代码中的 l 和 p 向量没有定义，我们这里提供一组示例值
//     // const l: [6]f64 = .{ 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 };
//     // const p: [3]f64 = .{ 7.0, 8.0, 9.0 };
//     const l: [6]f64 = .{ 0.36, 1.34, 0.491, 0, 0, 0 };
//     const p: [3]f64 = .{ -0.1, -2.3, 8.9 };
//     const t = threed_geometry_tool.line_point_nearest_2_point(l[0..6], p[0..3]);

//     // double t=point_nearest_2_line(p,l);
//     const point: [3]f64 = .{ t * l[0] + l[3], t * l[1] + l[4], t * l[2] + l[5] };

//     std.debug.print("rsult t:{} point:{} {} {}\n", .{ t, point[0], point[1], point[2] });

//     std.debug.print("dis:{}\n", .{lbmath.distance_of_two_points(p[0..3], point[0..3])});

//     const dir1: [3]f64 = .{ point[0] - p[0], point[1] - p[1], point[2] - p[2] };

//     std.debug.print("delta:{}\n", .{lbmath.inner_product(dir1[0..3], l[0..3])});
//     std.debug.print("end test convec1\n", .{});

//     // // 2. 根据输入数据构建二次规划问题
//     const p1: [3]f64 = .{ l[3], l[4], l[5] }; // { 4.0, 5.0, 6.0 }
//     const p2: [3]f64 = .{ l[0] + l[3], l[1] + l[4], l[2] + l[5] }; // { 5.0, 7.0, 9.0 }

//     // // H 矩阵 (2x2)
//     var H_buffer: [2][2]f64 = undefined;
//     const ip_p1_p1 = lbmath.inner_product(&p1, &p1);
//     const ip_p1_p2 = lbmath.inner_product(&p1, &p2);
//     const ip_p2_p2 = lbmath.inner_product(&p2, &p2);

//     H_buffer[0][0] = ip_p1_p1 * 2.0;
//     H_buffer[0][1] = ip_p1_p2 * 2.0;
//     H_buffer[1][0] = ip_p1_p2 * 2.0; // 修正 C 代码中的笔误，使其对称
//     H_buffer[1][1] = ip_p2_p2 * 2.0;

//     // // C 向量 (长度为 2)
//     const C_buffer: [2]f64 = .{
//         -2.0 * lbmath.inner_product(&p, &p1),
//         -2.0 * lbmath.inner_product(&p, &p2),
//     };

//     // // 不等式约束 A*x <= b (不存在)
//     const A: ?[]const []const f64 = null;
//     const b: ?[]const f64 = null;

//     // // 等式约束 E*x = d
//     const E_buffer: [1][2]f64 = .{.{ 1.0, 1.0 }};
//     const d_buffer: [1]f64 = .{1.0};

//     // // 3. 将数组转换为函数所需的切片 (slices)
//     // // 这是 Zig 的惯用做法，因为函数通常接收切片而不是固定大小的数组
//     var H_rows: [H_buffer.len][]const f64 = .{ &H_buffer[0], &H_buffer[1] };
//     var E_rows: [E_buffer.len][]const f64 = .{&E_buffer[0]};

//     const H: []const []const f64 = &H_rows;
//     const C: []const f64 = &C_buffer;
//     const E: ?[]const []const f64 = &E_rows;
//     const d: ?[]const f64 = &d_buffer;

//     // // 4. 定义输出向量并调用函数
//     var x: [2]f64 = .{ 0.0, 0.0 }; // 初始猜测为 0
//     const allocator = std.testing.allocator;

//     const final_error = try optQuadraticProblemWithConstraint(
//         f64,
//         allocator,
//         H,
//         C,
//         A,
//         b,
//         E,
//         d,
//         &x,
//     );

//     // // 打印结果用于调试
//     std.debug.print("\n error: {}\n", .{final_error});
//     // std.debug.print("Solution x: {d:.4}, {d:.4}\n", .{ x[0], x[1] });

//     // // 5. 验证结果
//     // // 这是一个带有等式约束 x[0] + x[1] = 1 的二次规划问题。
//     // // 我们可以通过解析法求解 KKT 系统来得到精确解。
//     // // KKT 系统:
//     // // | H   E^T | | x | = | -C |
//     // // | E    0  | | λ |   |  d |
//     // // 求解后，理论上的精确解是 x = [-2/7, 9/7]
//     // const expected_x: [2]f64 = .{ -2.0 / 7.0, 9.0 / 7.0 };
//     // const tolerance = 1e-3; // 因为是迭代法，所以设置一个容差

//     // try std.testing.expectApproxEqAbs(expected_x[0], x[0], tolerance);
//     // try std.testing.expectApproxEqAbs(expected_x[1], x[1], tolerance);

//     // // 同时验证约束是否被满足
//     // try std.testing.expectApproxEqAbs(d_buffer[0], x[0] + x[1], tolerance);
// }

/// 不等式不可以转化为等式，
/// 等式可以消元，只能消除变量的维度，不能消除不等式的个数
/// ceoffs*x>=b
/// com是约束的组合，用几个bool为true的约束组合
/// 这些约束组合改为等式形成的线性子空间，改子空间是否
/// 和总约束的解空间有交集，返回的值>0没有交集，=0有交集
/// 比如：
/// 当com全为false,也就是没有约束组合时，反映总空间和总约束的解空间
/// 是否有交集，返回的值>0就是约束的解空间为空集，该总约束冲突，没有解。
/// =0表示有解。
/// 当com有些为true时，也就是这些com为等式约束形成的“棱”，
/// “面”，“角”是否和总约束有交集，也就是这些“棱”“面”，“角”
/// 是否属于构成解空间的组成部分。>0不构成，=0构成。
/// 自然可以用来判断某约束是否冗余。true 表示=0，false
/// 表示>0
pub fn compute_inequality_constraints_eigenvalue(
    coeffs: [][]f64,
    b: []f64,
    com: []bool,
    allocator: std.mem.Allocator,
) !bool {
    const n = coeffs.len;
    const m = coeffs[0].len;
    std.debug.assert(com.len == n);
    const coeffs1 = try allocator.alloc([]f64, m);
    for (0..m) |i| {
        coeffs1[i] = try allocator.alloc(f64, n);
        for (0..n) |j| {
            coeffs1[i][j] = coeffs[j][i];
        }
    }
    defer {
        for (0..m) |i| {
            allocator.free(coeffs1[i]);
        }
        allocator.free(coeffs1);
    }
    const b1 = try allocator.alloc(f64, m);
    defer allocator.free(b1);
    @memset(b1, 0);
    const x = try allocator.alloc(bool, n);
    defer allocator.free(x);
    // @memset(dest, elem)
    _ = lbmatrix.diagonalizable_matrix(f64, coeffs1, b1, null, x);
    var num: usize = 0;
    for (x) |xv| {
        if (xv) {
            num += 0;
        }
    }
    const repre = try allocator.alloc([]f64, n);
    for (0..n) |i| {
        repre[i] = try allocator.alloc(f64, num);
    }
    defer {
        for (0..n) |i| {
            allocator.free(repre[i]);
        }
        allocator.free(repre);
    }
    const repre_b = try allocator.alloc(f64, n);
    defer allocator.free(repre_b);
    lbmatrix.solve_Ax_b(f64, coeffs1, b1, x, repre, repre_b);

    const temp1 = try allocator.alloc(f64, num);
    defer allocator.free(temp1);
    try lbmatrix.arry_mult_matrix(b, repre, temp1);

    const temp2 = try allocator.alloc(f64, n);
    defer allocator.free(temp2);

    try lbmatrix.matrix_mult_array(repre, temp1, temp2);
    for (0..n) |i| {
        if (!com[i]) {
            if (temp2[i] < 0) {
                return true;
            }
        }
    }
    return false;
}
